# -*- coding: utf-8 -*-
"""
Created on Sun Jul 16 17:17:13 2017

@author: decardin
"""

import numpy as np
import Stream
import PCCPlant
from copy import copy
import mpctools as mpc
from casadi import *

pccplant = PCCPlant.PCCPlant()



### Symbolic variables for DAE system
x = SX.sym('x',103)
z = SX.sym('z', 14)
u = SX.sym('u',2)
d = SX.sym('d')
#
ode = pccplant.getODE(x, z, u)
alg = pccplant.getALG(x, z)
dae = {'x':x, 'z':z, 'p':vertcat(u) ,'ode':ode, 'alg':alg}
opts = {"tf":1, "linear_solver":"csparse", "regularity_check":0} # interval length
f = integrator('f', 'idas', dae, opts)

U = np.array([6.292384000000001E-1, 0.120])
x0 = np.array([0+1e-100,0+1e-100,0+1e-100,0+1e-100,0+1e-100,1.36577367410828,1.52492119508401,1.78280415583684,
               2.05841878937848,2.30778204516798,5.00037777111685,
               5.00091648881199,5.00121532177936,5.00073370395002,
               4.99931754412261,39.2520427439715,39.4668579191297,
               39.5836176999860,39.4172823768912,38.7790070280395,
               317.855650940145,323.958593146384,330.921748105085,
               334.847512762899,332.616814888341,0.0357100000000000,
               0.0357100000000000,0.0357100000000000,0.0357100000000000,
               0.0357100000000000,0.000642310354297167,0.00121595345281533,
               0.00232996430901564,0.00413514832801159,0.00606445566096361,
               4.77730662443188e-06,7.42168321763430e-06,1.11926635126786e-05,
               1.32844589863012e-05,9.91312212420876e-06,0.00329699007655575,
               0.00443128096060410,0.00593497174023011,0.00675227875548204,
               0.00558792782485725,318.901256882791,325.014061867868,
               331.387183355481,334.300226743961,330.820034569207,0+1e-100,0+1e-100,0+1e-100,0+1e-100,0+1e-100,
               2.19910009929652,2.07985766042335,1.94090111959926,
               1.78077116689360,1.60425563088652,5.00130844668470,
               5.00418452570399,5.00772893410805,5.01084473694572,
               5.00801637729185,39.1705616348130,39.7180021465625,
               40.3932619230571,41.1467240043108,41.9094865659001,
               360.100485113559,363.692123680230,368.209036285769,
               373.087657026185,377.901845284705,0+1e-100,0+1e-100,0+1e-100,0+1e-100,0+1e-100,0.00780416745856682,
               0.00695286685426053,0.00601884410979797,0.00493040126041845,
               0.00367610872828548,6.98904307006594e-05,8.54850951505934e-05,
               0.000108013340881340,0.000135776582432504,0.000160182555515342,
               0.0193087669768978,0.0223757985903026,0.0266638829352229,
               0.0319531688237476,0.0378550020344383,360.921156275616,
               364.675282603287,369.252020308032,374.236606363397,
               379.893275214802,364.734016024360,357.981291501000,
               390.000009571286])
z0 = np.array([0+1e-100, 0.0147604103,0.0504165221,0.934823068,0+1e-100,0.0841595048, 
      0.000826652134,0.915013843,0+1e-100,2.31465854662427,0.0201259574490178, 
      1.06789481184371,0.299649717, 1.36])

#r = f(x0=x0, z0=vertcat(*z0), p = vertcat(U, 0))
#print(r['xf'])
#print(r['zf'])

#xxz = f.jacobian()
#xxx = xxz(x0, vertcat(U,0), z0, 0, 0, 0)

Nx = x0.size
Nu = 2
Nd = 1
Nz = z0.size
#
nn = 36000
xx = np.zeros((nn,Nx))
zz = np.zeros((nn,Nz))
xx[0] = x0
zz[0] = z0
#
for i in range(1,nn):
    print '========================================'
    print i
    xnext = f(x0=xx[i-1], z0=vertcat(*zz[i-1]), p = vertcat(U))
    xx[i] = xnext['xf'].T
    zz[i] = xnext['zf'].T
#    print f.jacobian()
print(xnext['xf'])
##    # print(sim.sim(x0, 0, 0))
##    xf = sim.sim(xx[i-1], U, 0)
#
##    xx[i] = xnext
print(xnext['zf'])

xs = np.array([1e-100, 1e-100, 1e-100, 1e-100, 1e-100, 0.999888, 1.08069, 1.25515, 1.59241, 2.03867, 5.00023, 5.00066, 5.00117, 5.00102, 4.99943, 39.1918, 39.3702, 39.5734, 39.5253, 38.8241, 316.157, 320.166, 326.426, 333.106, 334.364, 0.03571, 0.03571, 0.03571, 0.03571, 0.03571, 0.000272533, 0.000539481, 0.00110488, 0.00232587, 0.00468641, 4.02047e-006, 5.6484e-006, 8.63964e-006, 1.22149e-005, 1.11632e-005, 0.00298177, 0.00369436, 0.0049434, 0.00636539, 0.0060287, 316.9, 321.204, 327.35, 333.003, 332.348, 1e-100, 1e-100, 1e-100, 1e-100, 1e-100, 1.96873, 1.86892, 1.74084, 1.59471, 1.44634, 5.0018, 5.00503, 5.00925, 5.01506, 5.02682, 39.2655, 39.8343, 40.513, 41.2492, 41.9622, 362.591, 366.789, 371.522, 376.444, 381.166, 7.39072e-109, 7.39072e-109, 7.39072e-109, 7.39072e-109, 7.39072e-109, 0.00888435, 0.00828127, 0.00742071, 0.00631649, 0.00505661, 8.05618e-005, 0.000101039, 0.000128915, 0.000165287, 0.00021536, 0.0212335, 0.0250388, 0.0299427, 0.0357928, 0.0421388, 363.447, 367.729, 372.487, 377.441, 382.632, 365.715, 359.118, 390.377])
zs = np.array([2.39873e-102, 0.0230225, 0.119601, 0.857376, 1.41102e-107, 0.0721188, 0.00604604, 0.921835, 5.88235e-006, 3.13253, 0.0505515, 1.07518, 0.139291, 0.961728])